#include <gtest/gtest.h>
#include <gmock/gmock.h>

#include "axisgroup/axisgroup.h"
#include "axisgroup/axisgroup_robot.h"
#include "axisgroup/fsm_disable.h"
#include "axisgroup/fsm_booting.h"
#include "axis/axis.h"

using namespace axisgroup;

class MockDisableAxis : public axis::Axis
{
public:
    MOCK_METHOD0(run_state, RunState());
    MOCK_METHOD0(get_id, int());
    MOCK_METHOD0(is_torque_over, bool());
};

class MockAg : public AxisGroup
{
public:
    FsmDisable* dis_sta;

    void init_fsm() override { fsm_sta_disabled_.reset(dis_sta); }
};

class FsmDisableTest : public ::testing::Test
{
protected:
    virtual void SetUp() override
    {
        ag = std::make_shared<MockAg>();
        dis_sta = new FsmDisable(ag.get());
        ag->dis_sta = dis_sta;
        ag->init_fsm();

        for (int i = 0; i < 6; i++)
        {
            EXPECT_CALL(j[i], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Disable));
            EXPECT_CALL(j[i], get_id()).WillRepeatedly(testing::Return(i));
            ag->link_joint(j + i);
        }
        ag->update_fsm();  //checkjoint
        ag->update_fsm();  //syncboot
        ag->update_fsm();  //syncdh
        ag->update_fsm();  //syncdyn
    }
    virtual void TearDown() override {}

    std::shared_ptr<MockAg> ag;
    FsmDisable* dis_sta;
    MockDisableAxis j[7];
};

TEST_F(FsmDisableTest, check_joint_err)
{
    EXPECT_EQ(ag->get_state(), axisgroup::AxisGroup::FsmState::Disabled);
    ag->update_fsm();
    EXPECT_EQ(ag->get_state(), axisgroup::AxisGroup::FsmState::Disabled);

    EXPECT_CALL(j[0], run_state()).WillOnce(testing::Return(axis::Axis::RunState::Disable));
    EXPECT_CALL(j[1], run_state()).WillOnce(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[2], run_state()).WillOnce(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[3], run_state()).WillOnce(testing::Return(axis::Axis::RunState::ErrorStop));
    EXPECT_CALL(j[4], run_state()).Times(0);
    EXPECT_CALL(j[5], run_state()).Times(0);
    ag->update_fsm();
    EXPECT_EQ(ag->get_state(), axisgroup::AxisGroup::FsmState::ErrorStop);
}

TEST_F(FsmDisableTest, enable_all_Standstill)
{
    ag->update_fsm();
    ag->do_enable(true);
    dis_sta->allow_enable_timer_ = 0;

    EXPECT_CALL(j[0], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[1], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[2], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Disable));
    EXPECT_CALL(j[3], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[4], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Disable));
    EXPECT_CALL(j[5], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[0], is_torque_over()).Times(0);
    ag->update_fsm();
    EXPECT_EQ(ag->get_state(), axisgroup::AxisGroup::FsmState::Disabled);

    EXPECT_CALL(j[0], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[1], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[2], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[3], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[4], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[5], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[0], is_torque_over()).WillOnce(testing::Return(false));
    ag->update_fsm();
    EXPECT_EQ(dis_sta->sta_has_enabled, true);
    EXPECT_EQ(ag->get_state(), axisgroup::AxisGroup::FsmState::Disabled);
}
TEST_F(FsmDisableTest, enable_timeout)
{
    ag->update_fsm();
    ag->do_enable(true);
    dis_sta->allow_enable_timer_ = 0;

    EXPECT_CALL(j[0], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[1], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[2], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Disable));
    EXPECT_CALL(j[3], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[4], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Disable));
    EXPECT_CALL(j[5], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[0], is_torque_over()).Times(0);
    ag->update_fsm();
    EXPECT_EQ(ag->get_state(), axisgroup::AxisGroup::FsmState::Disabled);

    dis_sta->enabling_timer_ = 16;
    ag->update_fsm();
    EXPECT_EQ(ag->get_state(), axisgroup::AxisGroup::FsmState::Disabled);
    EXPECT_EQ(dis_sta->enabling_timer_, 8);

    ag->update_fsm();
    EXPECT_EQ(ag->get_state(), axisgroup::AxisGroup::FsmState::Disabled);
    EXPECT_EQ(dis_sta->enabling_timer_, 0);

    ag->update_fsm();
    EXPECT_EQ(ag->get_state(), axisgroup::AxisGroup::FsmState::ErrorStop);
}

TEST_F(FsmDisableTest, enable_checkpayload_err)
{
    ag->update_fsm();
    ag->do_enable(true);
    dis_sta->allow_enable_timer_ = 0;

    EXPECT_CALL(j[0], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[1], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[2], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[3], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[4], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[5], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[0], is_torque_over()).WillRepeatedly(testing::Return(false));
    EXPECT_CALL(j[1], is_torque_over()).WillRepeatedly(testing::Return(false));
    EXPECT_CALL(j[2], is_torque_over()).WillRepeatedly(testing::Return(false));
    EXPECT_CALL(j[3], is_torque_over()).WillRepeatedly(testing::Return(false));
    EXPECT_CALL(j[4], is_torque_over()).WillRepeatedly(testing::Return(false));
    EXPECT_CALL(j[5], is_torque_over()).WillRepeatedly(testing::Return(false));

    ag->update_fsm();
    EXPECT_EQ(dis_sta->sta_has_enabled, true);
    EXPECT_EQ(ag->get_state(), axisgroup::AxisGroup::FsmState::Disabled);

    EXPECT_CALL(j[5], is_torque_over()).WillRepeatedly(testing::Return(true));
    ag->update_fsm();
    EXPECT_EQ(dis_sta->sta_has_enabled, true);
    EXPECT_EQ(ag->get_state(), axisgroup::AxisGroup::FsmState::ErrorStop);
}

TEST_F(FsmDisableTest, enable_checkpayload)
{
    ag->update_fsm();
    dis_sta->allow_enable_timer_ = 0;
    ag->do_enable(true);

    EXPECT_CALL(j[0], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[1], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[2], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[3], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[4], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[5], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    EXPECT_CALL(j[0], is_torque_over()).WillRepeatedly(testing::Return(false));
    EXPECT_CALL(j[1], is_torque_over()).WillRepeatedly(testing::Return(false));
    EXPECT_CALL(j[2], is_torque_over()).WillRepeatedly(testing::Return(false));
    EXPECT_CALL(j[3], is_torque_over()).WillRepeatedly(testing::Return(false));
    EXPECT_CALL(j[4], is_torque_over()).WillRepeatedly(testing::Return(false));
    EXPECT_CALL(j[5], is_torque_over()).WillRepeatedly(testing::Return(false));
    for (int i = 0; i < 200; i++)
    {
        ag->update_fsm();
        EXPECT_EQ(dis_sta->sta_has_enabled, true) << i;
        EXPECT_EQ(ag->get_state(), axisgroup::AxisGroup::FsmState::Disabled) << i;
    }

    ag->update_fsm();
    EXPECT_EQ(dis_sta->sta_has_enabled, true);
    EXPECT_EQ(ag->get_state(), axisgroup::AxisGroup::FsmState::Standby);
}